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Abstract 

This  paper  describes  a  scalable  algorithm  for  the  simultaneous  mapping  and  localization 
(SLAM)  problem.  SLAM  is  the  problem  of  determining  the  location  of  environmental  fea¬ 
tures  with  a  roving  robot.  Many  of  today’s  popular  techniques  are  based  on  extended  Kalman 
filters  (EKFs),  which  require  update  time  quadratic  in  the  number  of  features  in  the  map.  This 
paper  develops  the  notion  of  sparse  extended  information  filters  (SEIFs),  as  a  new  method  for 
solving  the  SLAM  problem.  SEIFs  exploit  structure  inherent  in  the  SLAM  problem,  represent¬ 
ing  maps  through  local.  Web-like  networks  of  features.  By  doing  so,  updates  can  be  performed 
in  constant  time,  irrespective  of  the  number  of  features  in  the  map.  This  paper  presents  sev¬ 
eral  original  constant-time  results  of  SEIFs,  and  provides  simulation  results  that  show  the  high 
accuracy  of  the  resulting  maps  in  comparison  to  the  computationally  more  cumbersome  EKF 
solution. 
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1  Introduction 


The  simultaneous  localization  and  mapping  (SLAM)  problem  is  the  problem  of  acquiring  a  map 
of  an  unknown  environment  with  a  moving  robot,  while  simultaneously  localizing  the  robot 
relative  to  this  map  [6,  12].  The  SLAM  problem  addresses  situations  where  the  robot  lacks  a 
global  positioning  sensor,  and  instead  has  to  rely  on  a  sensor  of  incremental  ego-motion  for 
robot  position  estimation  (e.g.,  odometry,  inertial  navigation).  Such  sensors  accumulate  error 
over  time,  making  the  problem  of  acquiring  an  accurate  map  a  challenging  one.  Within  mobile 
robotics,  the  SLAM  problem  is  often  referred  to  as  one  of  the  most  challenging  ones  [28]. 

In  recent  years,  the  SLAM  problem  has  received  considerable  attention  by  the  scientific 
community,  and  a  flurry  of  new  algorithms  and  techniques  has  emerged,  as  attested,  for  exam¬ 
ple,  by  a  recent  workshop  on  this  topic  [11].  Existing  algorithms  can  be  subdivided  into  batch 
and  online  techniques.  The  former  provide  sophisticated  techniques  to  cope  with  perceptual 
ambiguities  [2,  24,  30],  but  they  can  only  generate  maps  after  extensive  batch  processing.  On¬ 
line  techniques  are  specifically  suited  to  acquire  maps  as  the  robot  navigates  [6,  27],  which  is 
of  great  practical  importance  in  many  navigation  and  exploration  problems  [25].  Today’s  most 
widely  used  online  algorithms  are  based  on  extended  Kalman  filters  (EKFs),  based  on  a  semi¬ 
nal  series  of  papers  [17,  18,  27,  26].  EKFs  calculate  Gaussian  posteriors  over  the  locations  of 
environmental  features  and  the  robot  itself. 

A  key  bottleneck  of  EKFs — which  has  been  subject  to  intense  research — is  their  computa¬ 
tional  complexity.  The  standard  EKF  approach  requires  time  quadratic  in  the  number  of  features 
in  the  map,  for  each  incremental  update.  This  computational  burden  restricts  EKFs  to  relatively 
sparse  maps  with  no  more  than  a  few  hundred  features.  Recently,  several  researchers  have 
developed  hierarchical  techniques  that  decompose  maps  into  collections  of  smaller,  more  man¬ 
ageable  submaps  [1,  8,  31].  While  in  principle,  hierarchical  techniques  can  solve  this  problem 
in  linear  time,  most  of  these  techniques  still  require  quadratic  time  per  update.  However,  they 
do  so  with  a  much  reduced  constant  factor,  enabling  them  to  manage  significantly  more  fea¬ 
tures.  One  recent  technique  updates  the  estimate  in  constant  time  [13],  but  with  a  loss  of  global 
consistency  that  is  particularly  troublesome  when  closeing  loops  in  cyclic  environments  [9].  A 
different  line  of  research  has  relied  on  particle  filters  for  efficient  mapping  [7].  The  FastSLAM 
algorithm  [16]  and  related  mapping  algorithms  [19]  require  time  logarithmic  in  the  number  of 
features  in  the  map,  but  they  depend  linearly  on  a  particle-filter  specific  parameter  (the  number 
of  particles),  whose  scaling  with  environmental  size  is  still  poorly  understood.  None  of  these 
approaches,  however,  offer  constant  time  updating  while  simultaneously  maintaining  global 
consistency  of  the  map. 

This  paper  proposes  a  new  SLAM  algorithm  whose  updates  require  constant  time,  indepen¬ 
dent  of  the  number  of  features  in  the  map.  Our  approach  is  based  on  the  well-known  information 
form  of  the  EKF,  also  known  as  the  extended  information  filter  (EIF)  [22] .  To  achieve  constant 
time  updating,  we  develop  an  approximate  EIF  which  maintains  a  sparse  representation  of 
environmental  dependencies.  Empirical  simulation  results  provide  evidence  that  the  resulting 
maps  are  comparable  in  accuracy  to  the  computationally  much  more  cumbersome  EKF  solution, 
which  is  still  at  the  core  of  most  work  in  the  field. 
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Our  approach  is  best  motivated  by  investigating  the  workings  of  the  EKF.  Figure  1  shows  the 
result  of  EKF  mapping  in  an  environment  with  50  landmarks.  The  left  panel  shows  a  moving 
robot,  along  with  its  Gaussian  estimates  of  the  location  of  all  50  point  features.  The  central 
information  maintained  by  the  EKF  solution  is  a  covariance  matrix  of  these  different  estimates. 
The  normalized  covariance,  i.e.,  the  correlation,  is  visualized  in  the  center  panel  of  this  figure. 
Each  of  the  two  axes  lists  the  robot  pose  (x-y  location  and  orientation)  followed  by  the  x-y- 
locations  of  the  50  landmarks.  Dark  entries  indicate  strong  correlations.  It  is  known  that  in 
the  limit  of  SEAM,  all  ^-coordinates  and  all  ^-coordinates  become  fully  correlated  [6].  The 
checkerboard  appearance  of  the  correlation  matrix  illustrates  this  fact.  Maintaining  these  cross¬ 
correlations — of  which  there  are  quadratically  many  in  the  number  of  features  in  the  map — are 
essential  to  the  SLAM  problem.  This  observation  has  given  rise  to  the  (false)  suspicion  that 
online  SLAM  is  inherently  quadratic  in  the  number  of  features  in  the  map. 

The  key  insight  that  motivates  our  approach  is  shown  in  the  right  panel  of  Figure  1.  Shown 
there  is  the  inverse  covariance  matrix  (also  known  as  information  matrix  [15,  22]),  normalized 
just  like  the  correlation  matrix.  Elements  in  this  normalized  information  matrix  can  be  thought 
of  as  constraints,  or  links,  between  the  locations  of  different  features:  The  darker  an  entry  in 
the  display,  the  stronger  the  link.  As  this  depiction  suggests,  the  normalized  information  matrix 
appears  to  be  naturally  sparse:  it  is  dominated  by  a  small  number  of  strong  links,  and  possesses  a 
large  number  of  links  whose  values,  when  normalized,  are  near  zero.  Furthermore,  link  strength 
is  related  to  distance  of  features:  Strong  links  are  found  only  between  geometrically  nearby 
features.  The  more  distant  two  landmarks,  the  weaker  their  link.  This  observation  suggest  that 
the  EKF  solution  to  SLAM  possesses  important  structure  that  can  be  exploited  for  more  efficient 
solutions.  While  any  two  features  are  fully  correlated  in  the  limit,  the  correlation  arises  mainly 
through  a  network  of  local  links,  which  only  connect  nearby  landmarks. 

Our  approach  exploits  this  structure  by  maintaining  a  sparse  information  matrix,  in  which 
only  nearby  features  are  linked  through  a  non-zero  element.  The  resulting  network  structure  is 
illustrated  in  the  right  panel  of  Figure  2,  where  disks  corresponds  to  point  features  and  dashed 
arcs  to  links,  as  specified  in  the  information  matrix  visualized  on  the  left.  Shown  also  is  the 
robot,  which  is  linked  to  a  small  subset  of  all  features  only,  called  active  features  and  drawn 
in  black.  Storing  a  sparse  information  matrix  requires  linear  space.  More  importantly,  up¬ 
dates  can  be  performed  in  constant  time,  regardless  of  the  number  of  features  in  the  map.  The 
resulting  filter  is  a  sparse  extended  information  filter,  or  SEIF.  We  show  empirically  that  the 
SEIFs  tightly  approximate  conventional  extended  information  filters,  which  previously  applied 
to  SLAM  problems  in  [20,  22]  and  which  are  functionally  equivalent  to  the  popular  EKF  solu¬ 
tion. 

Our  technique  is  probably  most  closely  related  to  work  on  SLAM  filters  that  represent 
relative  distances,  such  as  Newman’s  geometric  projection  filter  [23]  and  extensions  [5],  and 
Csorba’s  relative  filter  [4].  Neither  of  these  alternative  approaches  permits  constant  time  up¬ 
dating  in  SLAM,  though  it  appears  that  these  techniques  could  be  developed  into  constant  time 
algorithms,  using  approximations  similar  to  the  ones  described  here.  Our  work  is  also  related 
to  the  rich  body  of  literature  on  topological  mapping  [3,  10,  14,  32],  which  typically  does  not 
represent  about  dependencies  and  correlations  in  the  representation  of  uncertainty. 
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Figure  1:  Typical  snapshots  of  EKFs  applied  to  the  SLAM  problem:  Shown  here  is  a  map  (left  panel),  a  correlation 
(center  panel),  and  a  normalized  information  matrix  (right  panel).  Notice  that  the  normalized  information  matrix 
is  naturally  almost  sparse,  motivating  our  approach  of  using  sparse  information  matrices  in  SLAM. 
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Figure  2:  Illustration  of  the  network  of  landmarks  generated  by  our  approach.  Shown  on  the  left  is  a  sparse 
information  matrix,  and  on  the  right  a  map  in  which  entities  are  linked  whose  information  matrix  element  is  non¬ 
zero.  As  argued  in  the  paper,  the  fact  that  not  all  landmarks  are  connected  is  a  key  structural  element  of  the  SLAM 
problem,  and  at  the  heart  of  our  constant  time  solution. 

The  remainder  of  this  paper  is  organized  as  follows.  Section  2  introduces  the  extended 
information  filter  (EIF),  which  forms  the  basis  of  our  approach.  This  approach  is  not  new,  al¬ 
though  the  literature  appears  to  lack  a  similarly  compact  derivation  of  EIFs.  Building  on  this, 
Section  3  introduces  sparse  SEIFs.  First  we  provide  three  constant-time  results  in  Section  3.1. 
In  particular,  we  show  that  all  filter  updates  can  be  carried  out  in  constant  time  if  the  informa¬ 
tion  matrix  is  sparse.  This  result  is  somewhat  surprising,  as  a  naive  implementation  of  motion 
updates  in  information  filters  require  inversion  of  the  entire  information  matrix,  an  0(N3)  op¬ 
eration.  Section  3.2  describes  an  amortized  constant-time  algorithm  for  recovering  EKF-style 
state  estimates,  needed  for  the  linearization  of  non-linear  motion  and  measurement  functions. 
Finally,  Section  3.3  describes  our  technique  for  enforcing  sparseness  in  SEIFs.  Experimental 
results  are  provided  in  Section  4,  we  we  specifically  compare  our  new  approach  to  the  EKF 
solution.  These  results  suggest  that  the  sparseness  constraint  introduces  only  very  small  errors 
in  the  resulting  maps,  when  compared  to  the  computationally  more  cumbersome  EKF  solution. 
However,  these  empirical  results  are  limited  in  that  they  are  based  on  simulated  data  only,  and 
they  do  not  address  data  association  problems  that  inherently  arise  in  real-world  SLAM. 

2  Extended  Information  Filters 

This  section  reviews  the  extended  information  filter  (EIF),  which  forms  the  basis  of  our  work. 
EIFs  are  computationally  equivalent  to  extended  Kalman  filters  (EKFs),  but  they  represent  in¬ 
formation  differently:  instead  of  maintaining  a  covariance  matrix,  the  EIF  maintains  an  inverse 
covariance  matrix,  also  known  as  information  matrix.  EIFs  have  previously  been  applied  to 
the  SLAM  problem,  most  notably  by  Nettleton  and  colleagues  [20,  22],  but  they  are  much  less 
common  than  the  EKF  approach. 

Most  of  the  material  in  this  section  applies  equally  to  linear  and  non-linear  filters.  We  have 
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chosen  to  present  all  material  in  the  extended,  non-linear  form,  since  robots  are  inherently  non¬ 
linear. 

2.1  Information  Form  of  the  SLAM  Problem 

Let  xt  denote  the  pose  of  the  robot  at  time  t.  For  rigid  mobile  robots  operating  in  a  planar  en¬ 
vironment,  the  pose  is  given  by  its  two  Cartesian  coordinates  and  the  robot’s  heading  direction. 
Let  N  denote  the  number  of  features  (e.g.,  landmarks)  in  the  environment.  The  variable  yn  with 
1  <  n  <  N  denotes  the  pose  of  the  n-th  feature.  For  example,  for  point  landmarks  in  the  plane, 
yn  may  comprise  the  two-dimensional  Cartesian  coordinates  of  this  landmark.  In  SLAM,  it  is 
usually  assumed  that  features  do  not  change  their  pose  (or  location)  over  time. 

The  robot  pose  xt  and  the  set  of  all  feature  locations  Y  together  constitute  the  state  of  the 
environment.  It  will  be  denoted  by  the  vector 

it  =  (  xt  Vi  ...  Vn  )  (1) 

where  superscript  T  refers  to  the  transpose  of  a  vector. 

In  the  SLAM  problem,  it  is  impossible  to  sense  the  state  directly — otherwise  there  would 

be  no  mapping  problem.  Instead,  the  robot  seeks  to  recover  a  probabilistic  estimate  of 
Written  in  a  Bayesian  form,  our  goal  shall  be  to  calculate  a  posterior  distribution  over  the  state 
it.  This  posterior 

P(it\zt,ut)  (2) 

is  conditioned  on  past  sensor  measurements  zl  =  zi,...,zt  and  past  controls  ul  = 

Sensor  measurements  zt  might,  for  example,  specify  the  approximate  range  and  bearing  to 
nearby  features.  Controls  ut  specify  the  robot  motion  command  asserted  in  the  time  interval 

(t  —  1;  t\. 

Following  the  rich  EKF  tradition  in  the  SLAM  literature,  our  approach  represents  the  pos¬ 
terior  p(ft  \  z1.  it1)  by  a  multivariate  Gaussian  distribution  over  the  state  £t.  The  mean  of  this 
distribution  will  be  denoted  // / ,  and  covariance  matrix  £t: 

p(it\zt,ut)  oc  exp{-§(&  -nt)TY,-l(it  -/it)}  (3) 

The  proportionality  sign  replaces  a  constant  normalizer  that  is  easily  recovered  from  the  covari¬ 
ance  £(.  The  representation  of  the  posterior  via  the  mean  //,  and  the  covariance  matrix  Yt  is  the 
basis  of  the  EKF  solution  to  the  SLAM  problem  (and  to  EKFs  in  general). 

Information  filters  represent  the  same  posterior  through  a  so-called  information  matrix  Ht 
and  an  information  vector  bt — instead  of  ptt  and  Yt.  These  are  obtained  by  multiplying  out  the 
exponent  of  (3): 

=  exp  {-|  [&T£t-1&  -  2  Pt^i'it  +  //fsrV  t\  } 

=  exp  {-li'?Zf1it  + (4) 
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We  now  observe  that  the  last  term  in  the  exponent,  —  Et  1  yt  does  not  contain  the  free  vari¬ 
able  and  hence  can  be  subsumed  into  the  constant  normalizer.  This  gives  us  the  form: 


=  :Ht  =:bt 


The  information  matrix  Ht  and  the  information  vector  bt  are  now  defined  as  indicated: 

Ht  =  St’1 
bt  =  njHt 


(5) 


(6) 

(7) 


Using  these  notations,  the  desired  posterior  can  now  be  represented  in  what  is  commonly  known 
as  the  information  form  of  the  Kalman  filter: 

P(tt\zt,ut)  oc  exp  Ht£t  +  btit)  (8) 

As  the  reader  may  easily  notice,  both  representations  of  the  multi- variate  Gaussian  posterior  are 
functionally  equivalent  (with  the  exception  of  certain  degenerate  cases):  The  EKF  representa¬ 
tion  of  the  mean  /q  and  covariance  S t,  and  the  EIF  representation  of  the  information  vector  bt 
and  the  information  matrix  Ht.  In  particular,  the  EKF  representation  can  be  ‘recovered’  from 
the  information  form  via  the  following  algebra: 

=  Hf1  (9) 

Ik  =  HflbTt  =  E  tbTt  (10) 


The  advantage  of  the  EIF  over  the  EKF  will  become  apparent  further  below,  when  the  concept 
of  sparse  EIFs  will  be  introduced. 

Of  particular  interest  will  be  the  geometry  of  the  information  matrix.  This  matrix  is  sym¬ 
metric  and  positive-definite: 


^  HXt,xt 

Hxt,yi 

HXt,yN  ^ 

Ht  = 

Hy\  ,xt 

TT 

TT 

TT 

11  yi  ,vn 

TT 

V  HyNtXt  HVn,  yi  '  '  ‘  HyN,UN  ) 


(11) 


Each  element  in  the  information  matrix  constraints  one  (on  the  main  diagonal)  or  two  (off  the 
main  diagonal)  elements  in  the  state  vector.  We  will  refer  to  the  off-diagonal  elements  as  links: 
the  matrices  HXtVn  link  together  the  robot  pose  estimate  and  the  location  estimate  of  a  specific 
feature,  and  the  matrices  Hyn<yw  for  n  f  n'  link  together  two  feature  locations  yn  and  ynt. 
Although  rarely  made  explicit,  the  manipulation  of  these  links  is  the  very  essence  of  Gaussian 
solutions  to  the  SLAM  problem.  It  will  be  an  analysis  of  these  links  that  ultimately  leads  to  a 
constant-time  solution  to  the  SLAM  problem. 


6 


Figure  3:  The  effect  of  measurements  on  the  information  matrix  and  the  associated  network  of  features:  (a) 
Observing  y\  results  in  a  modification  of  the  information  matrix  elements  HXuVl.  (b)  Similarly,  observing  y2 
affects  / I  f  1  .'1/2 .  Both  updates  can  be  carried  out  in  constant  time. 

2.2  Measurement  Updates 

In  SLAM,  measurements  zt  carry  spatial  information  on  the  relation  of  the  robot’s  pose  and  the 
location  of  a  feature.  For  example,  zt  might  be  the  approximate  range  and  bearing  to  a  nearby 
landmark.  Without  loss  of  generality,  we  will  assume  that  each  measurement  zt  corresponds  to 
exactly  one  feature  in  the  map.  Sightings  of  multiple  features  at  the  same  time  may  easily  be 
processed  one- after- another. 

Figure  3  illustrates  the  effect  of  measurements  on  the  information  matrix  Ht.  Suppose  the 
robot  measures  the  approximate  range  and  bearing  to  the  feature  y  1,  as  illustrated  in  Figure  3a. 
This  observation  links  the  robot  pose  xt  to  the  location  of  tj\.  The  strength  of  the  link  is  given 
by  the  level  of  noise  in  the  measurement.  Updating  EIFs  based  on  this  measurement  involves 
the  manipulation  of  the  off-diagonal  elements  HXt  y  and  their  symmetric  counterparts  Hy  xt 
that  link  together  xt  and  y.  Additionally,  the  on-diagonal  elements  IIXt.Xt  and  Hyi  ,yi  are  also 
updated.  These  updates  are  additive:  Each  observation  of  a  feature  y  increases  the  strength  of 
the  total  link  between  the  robot  pose  and  this  very  feature,  and  with  it  the  total  information  in 
the  filter.  Figure  3b  shows  the  incorporation  of  a  second  measurement  of  a  different  feature, 
y2.  In  response  to  this  measurement,  the  EIF  updates  the  links  HXty2  =  Hjn  xt  (and  HXuXt  and 
lly2,in)-  As  this  example  suggests,  measurements  introduce  links  only  between  the  robot  pose 
xt  and  observed  features.  Measurements  never  generate  links  between  pairs  of  landmarks,  or 
between  the  robot  and  unobserved  landmarks. 

For  a  mathematical  derivation  of  the  update  rule,  we  observe  that  Bayes  rule  enables  us  to 
factor  the  desired  posterior  (2)  into  the  following  product: 

P(£t  I  Au*)  oc  p(zt  |  p(&  | 

=  p(zt\£t)  p(it\zt~1,ut)  (12) 

The  second  step  of  this  derivation  exploited  common  (and  obvious)  independences  in  SLAM 
problems  [29].  For  the  time  being,  we  assume  that  p(£t  |  zp  1,ut)  is  represented  by  II,  and 
bt.  Those  will  be  discussed  in  the  next  section,  where  robot  motion  will  be  addressed.  The  key 
question  addressed  in  this  section,  thus,  concerns  the  representation  of  the  probability  distribu¬ 
tion  p(zt  j  () )  and  the  mechanics  of  carrying  out  the  multiplication  above.  In  the  ‘extended’ 
family  of  filters,  a  common  model  of  robot  perception  is  one  in  which  measurements  are  gov- 
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erned  via  a  deterministic  non-linear  measurement  function  h  with  added  Gaussian  noise: 


zt  —  h(£t)  +  £t  (13) 

Here  et  is  an  independent  noise  variable  with  zero  mean,  whose  covariance  will  be  denoted  Z. 
Put  into  probabilistic  terms,  (13)  specifies  a  Gaussian  distribution  over  the  measurement  space 
of  the  form 

p{zt  I  &)  oc  exp  {~l(zt  -  h{it))T  Z~l(zt  -  /&(&))} 

(14) 

Following  the  rich  literature  of  EKFs,  EIFs  approximate  this  Gaussian  by  linearizing  the  mea¬ 
surement  function  h.  More  specifically,  a  Taylor  series  expansion  of  h  gives  us 

h(£t)  ~  h(fit)  +  V ^h(fit)[^t  -  fit]  (15) 

where  is  the  first  derivative  (Jacobian)  of  h  with  respect  to  the  state  variable  £,  taken 

£  =  Ht-  F°r  brevity,  we  will  write  zt  =  //(/// )  to  indicate  that  this  is  a  prediction  given  our  state 
estimate  pt-  The  transpose  of  the  Jacobian  matrix  and  will  be  denoted  Ct.  With  these 

definitions,  Equation  (15)  reads  as  follows: 

h(£t)  ~  +  Cf (it  —  pt)  (16) 

This  approximation  leads  to  the  following  Gaussian  approximation  of  the  measurement  density 
(14): 

p(zt  \  it)  oc  exp  ^l(zt- zt-C^t  +  Cj'fit)TZ~1(zt-  zt  -  +  CtT/jt)} 

(17) 

Multiplying  out  the  exponent  and  regrouping  the  resulting  terms  gives  us 

=  exp  {-\gCtZ~lCjit  +  (zt  -zt  +  C?iit)TZ-lC^t  (18) 

-\(zt-zt  +  C^t)TZ-\zt  -  Zt  +  CtUt )} 

As  before,  the  final  term  in  the  exponent  does  not  depend  on  the  variable  and  hence  can  be 
subsumed  into  the  proportionality  factor: 

oc  exp  {-l&CtZ-'CZSt  +  (zt  -zt  +  (19) 

We  are  now  in  the  position  to  state  the  measurement  update  equation,  which  implement  the 
probabilistic  law  (12). 

p(&  i  yy 

oc  exp  {  -  Ht£t  +  hit } 

•  exp  CtZ~lCl it  +  (zt  -zt  +  C^t)TZ-'Cjit} 

—  exp {  —  ^i'[(Ht  +  CtZ  xCj)it  +  (bt  +  (zt  -zt  +  C%m)TZ  lCj)it\  (20) 

' - V - '  ' - V - ' 

Ht  bt 
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Figure  4:  The  effect  of  motion  on  the  information  matrix  and  the  associated  network  of  features:  (a)  before 
motion,  and  (b)  after  motion.  If  motion  is  non-deterministic,  motion  updates  introduce  new  links  for  reinforce 
existing  links)  between  any  two  active  features,  while  weakening  the  links  between  the  robot  and  those  features. 
This  step  introduces  links  between  pairs  of  landmarks. 

Thus,  the  measurement  update  of  the  EIF  is  given  by  the  following  additive  rule: 

Ht  =  Ht  +  CtZ~lCj  (21) 

bt  —  bt  +  (zt  —  Zt  +  Cj nt)T Z  lCj  (22) 

In  the  general  case,  these  updates  may  modify  the  entire  information  matrix  Ht  and  vector  bt, 
respectively.  A  key  observation  of  all  SLAM  problems  is  that  the  Jacobian  Ct  is  sparse.  In 
particular,  Ct  is  zero  except  for  the  elements  that  correspond  to  the  robot  pose  xt  and  the  feature 
yt  observed  at  time  t. 

C«  =  (£  o-o  t,  °-°)T  <23> 

This  sparseness  is  due  to  the  fact  that  measurements  zt  are  only  a  function  of  the  relative  distance 
and  orientation  of  the  robot  to  the  observed  feature.  As  a  pleasing  consequence,  the  update 
CtZ^Cf  to  the  information  matrix  in  (21)  is  only  non-zero  in  four  places:  the  off-diagonal 
elements  that  link  the  robot  pose  xt  with  the  observed  feature  yt,  and  the  main-diagonal  elements 
that  correspond  to  xt  and  yt.  Thus,  the  update  equations  (21)  and  (22)  are  well  in  tune  with  our 
intuitive  description  given  in  the  beginning  of  this  section,  where  we  argued  that  measurements 
only  strengthen  the  links  between  the  robot  pose  and  observed  features,  in  the  information 
matrix. 

To  compare  this  to  the  EKF  solution,  we  notice  that  even  though  the  change  of  the  informa¬ 
tion  matrix  is  local,  the  resulting  covariance  usually  changes  in  non-local  ways,  put  differently, 
the  difference  between  the  old  covariance  E,  =  II,  1  and  the  new  covariance  matrix  E,  =  Hf1 
is  usually  non-zero  everywhere. 

2.3  Motion  Updates 

The  second  important  step  of  SLAM  concerns  the  update  of  the  filter  in  accordance  to  robot  mo¬ 
tion.  In  the  standard  SLAM  problem,  only  the  robot  pose  changes  over  time.  The  environment 
is  static. 

The  effect  of  robot  motion  on  the  information  matrix  Ht  are  slightly  more  complicated  than 
that  of  measurements.  Figure  4a  illustrates  an  information  matrix  and  the  associated  network 
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before  the  robot  moves,  in  which  the  robot  is  linked  to  two  (previously  observed)  landmarks. 
If  robot  motion  was  free  of  noise,  this  link  structure  would  not  be  affected  by  robot  motion. 
However,  the  noise  in  robot  actuation  weakens  the  link  between  the  robot  and  all  active  features. 
Hence  HXuyi  and  HXtm  are  decreased  by  a  certain  amount.  This  decrease  reflects  the  fact  that 
the  noise  in  motion  induces  a  loss  of  information  of  the  relative  location  of  the  features  to  the 
robot.  Not  all  of  this  information  is  lost,  however.  Some  of  it  is  shifted  into  between-landmark 
links  Hyi  y2,  as  illustrated  in  Figure  4b.  This  reflects  the  fact  that  even  though  the  motion 
induced  a  loss  of  information  of  the  robot  relative  to  the  features,  no  information  was  lost 
between  individual  features.  Robot  motion,  thus,  has  the  effect  that  features  that  were  indirectly 
linked  through  the  robot  pose  become  linked  directly. 

To  derive  the  update  rule,  we  begin  with  a  Bayesian  description  of  robot  motion.  Updating 
a  filter  based  on  robot  motion  motion  involves  the  calculation  of  the  following  posterior: 

pft  I  ^_1X)  =  j pft  I  it- p{it- 1  |  d£t  (24) 

Exploiting  the  common  SLAM  independences  [29]  leads  to 

=  J  p(it  I  6-i ,ut)  p(it- 1  I  2t-1,ut_1)  d£t  (25) 

The  term  p(£t- i  I  zt_1,  uf~l)  is  the  posterior  at  time  t  —  1,  represented  by  Ht- 1  and  bt~ i.  Our 
concern  will  therefore  be  with  the  remaining  term  p(£t  |  which  characterizes  robot 

motion  in  probabilistic  terms. 

Similar  to  the  measurement  model  above,  it  is  common  practice  to  model  robot  motion  by 
a  non-linear  function  with  added  independent  Gaussian  noise: 

it  =  it-i  +  At  with  At  =  g(it-i,ut)  +  Sxdt  (26) 

Here  g  is  the  motion  model,  a  vector-valued  function  which  is  non-zero  only  for  the  robot  pose 
coordinates,  as  feature  locations  are  static  in  SLAM.  The  term  labeled  At  constitutes  the  state 
change  at  time  t.  The  stochastic  part  of  this  change  is  modeled  by  5t,  a  Gaussian  random  variable 
with  zero  mean  and  covariance  Ut.  This  Gaussian  variable  is  a  low-dimensional  variable  defined 
for  the  robot  pose  only.  Here  Sx  is  a  projection  matrix  of  the  form 

sx  =  (  /  0  . . .  0  )T  (27) 

where  /  is  an  identity  matrix  of  the  same  dimension  as  the  robot  pose  vector  xt  and  as  of  5t. 
Each  0  in  (27)  refers  to  a  null  matrix,  of  which  there  are  N  in  Sx.  The  product  SxSt,  hence,  give 
the  following  generalized  noise  variable,  enlarged  to  the  dimension  of  the  full  state  vector  £: 

SJt  =  (6t  0  ...  Of  (28) 

In  EILs,  the  function  g  in  (26)  is  approximated  by  its  first  degree  Taylor  series  expansion: 

g(it-i,ut)  ~  g(pt-i,ut)  +  V(g(iH-i,ut)[it-i- ih-i] 

=  At  +  Atit_i  —  (29) 
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Here  At  =  V^g(fit-i,  ut)  is  the  derivative  of  g  with  respect  to  £  at  £  =  and  ut.  The  symbol 
A*  is  short  for  the  predicted  motion  effect,  g(pt_1,  ut).  Plugging  this  approximation  into  (26) 
leads  to  an  approximation  of  £t,  the  state  at  time  t: 

£t  ~  (I  +  +  At  —  Atfit_i  +  S'aA  (30) 

Hence,  under  this  approximation  the  random  variable  is  again  Gaussian  distributed.  Its  mean 
is  obtained  by  replacing  £t  and  St  in  (30)  by  their  respective  means: 

ft  =  {I  +  At)  fit~ i  +  At  —  Atfit-i  +  Sx0 

=  fit- 1  +  At  (31) 


The  covariance  of  is  simply  obtained  by  scaled  and  adding  the  covariance  of  the  Gaussian 
variables  on  the  right-hand  side  of  (30): 

^  =  (/  +  At)T>t-i(I  +  At)T  +  0  —  0  +  SxUtSl 

=  (/  +  At)^t-i(I  +  At)T  +  SxUtSTx  (32) 


Update  equations  (31)  and  (32)  are  in  the  EKF  form,  that  is,  they  are  defined  over  means  and  co- 
variances.  The  information  form  is  now  easily  recovered  from  the  definition  of  the  information 
form  in  (6)  and  (7)  and  its  inverse  in  (9)  and  (10).  In  particular,  we  have 


Ht 


{I  +  At)Tjt-i{I  +  At)T  +  SxUtSTx 
(I  +  At)H-_\(I  +  At)T  +  SxUtSTx 


(33) 


and 


bt  =  fijHt 


fit-  1  +  At 


T  - 
Ht 


Ht\bTt_  i  +  At 


bt-\Ht_l  +  A 


Ht 

Ht 


(34) 


These  equations  appear  computationally  involved,  in  that  they  require  the  inversion  of  large 
matrices.  In  the  general  case,  the  complexity  of  the  EIF  is  therefore  cubic  in  the  size  of  the  state 
space.  In  the  next  section,  we  provide  the  surprising  result  that  both  Ht  and  bt  can  be  computed 
in  constant  time  if  Ht- 1  is  sparse. 


3  Sparse  Extended  Information  Filters 

The  central,  new  algorithm  presented  in  this  paper  is  the  Sparse  Extended  Information  Filter , 
or  SEIF.  SEIF  differ  from  the  extended  information  filter  described  in  the  previous  section  in 
that  is  maintains  a  sparse  information  matrix.  An  information  matrix  Ht  is  considered  sparse  if 
the  number  of  links  to  the  robot  and  to  each  feature  in  the  map  is  bounded  by  a  constant  that  is 
independent  of  the  number  of  features  in  the  map.  The  bound  for  the  number  of  links  between 
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the  robot  pose  and  other  features  in  the  map  will  be  denoted  9X;  the  bound  on  the  number  of 
links  for  each  feature  (not  counting  the  link  to  the  robot)  will  be  denoted  6y.  The  motivation  for 
maintaining  a  sparse  information  matrix  was  already  given  above:  In  SLAM,  the  normalized 
information  matrix  is  already  almost  sparse.  This  suggests  that  by  enforcing  sparseness,  the 
induced  approximation  error  is  small. 

3.1  Constant  Time  Results 

We  begin  by  proving  three  important  constant  time  results,  which  form  the  backbone  of  SEIFs. 
All  proofs  of  these  results  can  be  found  in  the  Appendix. 

Lemma  1:  The  measurement  update  in  Section  (2.2)  requires  constant  time,  irrespective  of 
the  number  of  features  in  the  map. 

This  lemma  ensures  that  measurements  can  be  incorporated  in  constant  time.  Notice  that 
this  lemma  does  not  require  sparseness  of  the  information  matrix;  rather,  it  is  a  well-known 
property  of  information  filters  in  SLAM. 

Less  trivial  is  the  following  lemma: 

Lemma  2:  If  the  information  matrix  is  sparse  and  At  =  0,  the  motion  update  in  Section  (2.2) 
requires  constant  time.  The  constant-time  update  equations  are  given  by: 

Lt  =  S^  +  SlHt^S^SlHt^ 

Ht  =  Ht-i  —  Ht_iLt  (35) 

bt  =  bt-i  +  A.J  Ht_i  —  bt~\Lt  +  A.J  Ht_iLt 

This  result  addresses  the  important  special  case  At  =  0,  that  is,  the  Jacobian  of  pose  change 
with  respect  to  the  absolute  robot  pose  is  zero.  This  is  the  case  for  robots  with  linear  mechanics, 
and  with  non-linear  mechanics  where  there  is  no  ‘cross-talk’  between  absolute  coordinates  and 
the  additive  change  due  to  motion. 

In  general,  At  ^  0,  since  the  x-y  update  depends  on  the  robot  orientation.  This  case  is 
addressed  by  the  next  lemma: 

Lemma  3:  If  the  information  matrix  is  sparse,  the  motion  update  in  Section  (2.2)  requires 

constant  time  if  the  mean  pt  is  available  for  the  robot  pose  and  all  active  landmarks.  The 

constant-time  update  equations  are  given  by: 

=  i-sM  +  is^As^y'sI 

HU  =  i*t 

A  Ht  =  HUSX[U^1  +  STxHUSx]-lSlHU 
Ht  =  HU-  AHt 

bt  =  bt^-pUUHt-Ht^  +  UU  +  KSt  (36) 

For  At  7^  0,  a  constant  time  update  requires  knowledge  of  the  mean  pt_i  before  the  mo¬ 
tion  command,  for  the  robot  pose  and  all  active  landmarks  (but  not  the  passive  features).  This 
information  is  not  maintained  by  the  standard  information  filter,  and  extracting  it  in  the  straight¬ 
forward  way  (via  Equation  (10))  requires  more  than  constant  time.  A  constant- time  solution  to 
this  problem  will  now  be  presented. 
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3.2  Amortized  Approximated  Map  Recovery 

Before  deriving  an  algorithm  for  recovering  the  state  estimate  fit  from  the  information  form, 
let  us  briefly  consider  what  parts  of  fit  are  needed  in  SEIFs,  and  when.  SEIFs  need  the  state 
estimate  / it  of  the  robot  pose  and  the  active  features  in  the  map.  These  estimates  are  needed  at 
three  different  occasions:  (1)  the  linearization  of  the  non-linear  measurement  and  motion  model, 
(2)  the  motion  update  according  to  Lemma  3,  and  (3)  the  sparsification  technique  described 
further  below.  For  linear  systems,  the  means  are  only  needed  for  the  sparsification  (third  point 
above).  We  also  note  that  we  only  need  constantly  many  of  the  values  in  /zt,  namely  the  estimate 
of  the  robot  pose  and  of  the  locations  of  active  features. 

As  stated  in  (10),  the  mean  vector  /zt  is  a  function  of  Ht  and  bt: 

fit  =  H~X  =  EtbJ  (37) 


Unfortunately,  calculating  (37)  directly  involves  inverting  a  large  matrix,  which  would  requires 
more  than  constant  time. 

The  sparseness  of  the  matrix  Ht  allows  us  to  recover  the  state  incrementally.  In  particu¬ 
lar,  we  can  do  so  on-line,  as  the  data  is  being  gathered  and  the  estimates  b  and  H  are  being 
constructed.  To  do  so,  it  will  prove  convenient  to  pose  (37)  as  an  optimization  problem: 

Lemma  4:  The  state  fit  is  the  mode  ut  :=  argmax v  p{yt)  of  the  Gaussian  distribution,  de- 
fi  ned  over  the  variable  if: 

p{vt)  =  const.  •  exp  ^  —  ^ufHtut  +  bjut}  (38) 

Here  ut  is  a  vector  of  the  same  form  and  dimensionality  as  Hr  This  lemma  suggests  that 
recovering  fit  is  equivalent  to  finding  the  mode  of  (38).  Thus,  it  transforms  a  matrix  inversion 
problem  into  an  optimization  problem.  For  this  optimization  problem,  we  will  now  describe 
an  iterative  hill  climbing  algorithm  which,  thanks  to  the  sparseness  of  the  information  matrix, 
requires  only  constant  time  per  optimization  update. 

Our  approach  is  an  instantiation  of  coordinate  descent.  For  simplicity,  we  state  it  here  for  a 
single  coordinate  only;  our  implementation  iterates  a  constant  number  K  of  such  optimizations 
after  each  measurement  update  step.  The  mode  ut  of  (38)  is  attained  at: 


ut  =  argmax  p(ut) 

ft 

=  argmax  exp  |  | vj Htut  +  bjut  \ 

=  argmin  \u[Htut-bJut  (39) 

ft 

We  note  that  the  argument  of  the  min-operator  in  (39)  can  be  written  in  a  form  that  makes  the 
individual  coordinate  variables  v%  i  (for  the  z-th  coordinate  of  ut)  explicit: 


W  Ht"t  -  bj  ut  =  IY.Y,  -  5]  l>It,G,t 

i  j  i 


(40) 


where  Hhht  is  the  element  with  coordinates  (i,j)  in  If  ,  and  bht  if  the  z-th  component  of  the 
vector  bt.  Taking  the  derivative  of  this  expression  with  respect  to  an  arbitrary  coordinate  variable 
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Figure  5:  Sparsifi  cation:  A  feature  is  deactivated  by  eliminating  its  link  to  the  robot.  To  compensate  for  this 
change  in  information  state,  links  between  active  features  and/or  the  robot  are  also  updated.  The  entire  operation 
can  be  performed  in  constant  time. 

»i,t  gives  US 


d 

dvij 


I E  E  - 

i  j 


'E  bit 

j 


(41) 


Setting  this  to  zero  leads  to  the  optimum  of  the  i-th  coordinate  variable  ui  t  given  all  other 
estimates  u]t\ 


''i.t 


=  H~it 


E-E^E, 

j¥=i 


(42) 


The  same  expression  can  conveniently  be  written  in  matrix  notation,  were  Si  is  a  projection 
matrix  for  extracting  the  z-th  component  from  the  matrix  //,: 


..[k+l] 

''i.t. 


bt  -  Htu\k]  +  HtSiS'i  Y 


T,,[fe] 


(43) 


All  other  estimates  //,/,/  with  %'  ^  i  remain  unchanged  in  this  update  step,  that  is,  i/jfE  =  1Jf\- 
As  is  easily  seen,  the  number  of  elements  in  the  summation  in  (42),  and  hence  the  vector 
multiplication  in  (43),  is  constant  if  Ht  is  sparse.  Hence,  each  update  requires  constant  time.  To 
maintain  the  constant-time  property  of  our  SLAM  algorithm,  we  can  afford  a  constant  number 
of  updates  K  per  time  step.  This  will  generally  not  lead  to  convergence,  but  the  relaxation 
process  takes  place  over  multiple  time  steps,  resulting  in  small  errors  in  the  overall  estimate. 


3.3  Sparsification 

The  final  step  in  SEIFs  concerns  the  sparsification  of  the  information  matrix  Ht.  Sparsification 
is  necessarily  an  approximative  step,  since  information  matrices  in  SLAM  are  naturally  not 
sparse — even  though  normalized  information  matrices  tend  to  be  almost  sparse.  In  the  context 
of  SLAM,  it  suffices  to  remove  links  (deactivate)  between  the  robot  pose  and  individual  features 
in  the  map;  if  done  correctly,  this  also  limits  the  number  of  links  between  pairs  of  features. 

To  see,  let  us  briefly  consider  the  two  circumstances  under  which  a  new  link  may  be  intro¬ 
duced.  First,  observing  a  passive  feature  activates  this  feature,  that  is,  introduces  a  new  link 
between  the  robot  pose  and  the  very  feature.  Thus,  measurement  updates  potentially  violate  the 
bound  9X.  Second,  motion  introduces  links  between  any  two  active  features,  and  hence  lead  to 
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violations  of  the  bound  9y.  This  consideration  suggests  that  controlling  the  number  of  active 
features  can  avoid  violation  of  both  sparseness  bounds. 

Our  sparsification  technique  is  illustrated  in  Figure  5.  Shown  there  is  the  situation  before  and 
after  sparsification.  The  removal  of  a  link  in  the  network  corresponds  to  setting  an  element  in 
the  information  matrix  to  zero;  however,  this  requires  the  manipulation  of  other  links  between 
the  robot  and  other  active  landmarks.  The  resulting  network  is  only  an  approximation  to  the 
original  one,  whose  quality  depends  on  the  magnitude  of  the  link  before  removal. 

We  will  now  present  a  constant-time  sparsification  technique.  To  do  so,  it  will  prove  useful 
to  partition  the  set  of  all  features  into  two  subsets: 

Y  =  y+yy°wy-  (44) 


where  Y+  is  the  set  of  all  active  features  that  shall  remain  active.  Y°  are  one  or  more  active 
features  that  we  seek  to  deactivate  (remove  the  link  to  the  robot).  Finally,  Y ~  are  all  currently 
passive  features. 

The  sparsification  is  best  derived  from  first  principles.  If  Y+  l±)  Y°  contains  all  currently 
active  features,  the  posterior  (2)  can  be  factored  as  follows: 

p(xt,Y  \  z^u*)  =  p(xt,Y°,Y+,Y-  |  z^u*) 

=  p(xt  |  Y°,Y+,Y-,zt,ut)p(Y°,Y+,Y-  j  z^u*) 

=  P(xt  |  y°,  y+,  y  =  o,  z\  «*)  p(y°,  y+,  y~  \  z\  u*)  (45) 


In  the  last  step  we  exploited  the  fact  that  if  we  know  the  active  features  Y°  and  Y+ ,  the  variable 
xt  does  not  depend  on  the  passive  features  Y~ .  We  can  hence  set  Y~  to  an  arbitrary  value 
without  affecting  the  conditional  posterior  over  xt,  p(xt  j  Y°,  Y'+,  Y'~ ,  zJ\  u1).  Here  we  simply 
chose  Y~  =  0. 

To  sparsify  the  information  matrix,  the  posterior  is  approximated  by  the  following  distribu¬ 
tion,  in  which  we  simply  drop  the  dependence  on  Y°  in  the  first  term.  It  is  easily  shown  that 
this  distribution  minimizes  the  KL  divergence  to  the  exact,  non-sparse  distribution: 


p{xt,Y  |  zt,ut) 


p{xt  |  y+,y~  =  o,  z*,  ub)  p(y°,y+,y- 
p(xt,Y+  |  y~  o  + 

p(Y+  \Y-  =0,zt,ut)  n  ’  ’ 


(46) 


This  posterior  is  calculated  in  constant  time.  In  particular,  we  begin  by  calculating  the  informa¬ 
tion  matrix  for  the  distribution  p(xt,  Y'°,  Y+  \  V'  =  0)  of  all  variables  but  Y'~,  and  conditioned 
on  Y~  =  0.  This  is  obtained  by  extracting  the  submatrix  of  all  state  variables  but  Y~: 


K  =  Sx,Y+,yoSl  y+,Y°  HtSX:Y+,YoS^ 

Y+Y° 


(47) 


With  that,  the  onversion  lemma  leads  to  the  following  information  matrices  for  the  terms 
p(xt,  Y+  |  y-  =  0,  zf,  u*)  and  p(Y+  \  Y~  =  0,  u*),  denoted  H]  and  H^,  respectively: 

TT 1  Tj!  TTl  Q  (  QT  Tj!  Q  \—\qT  Tt! 

-  Ht  -  HtbY0(OYoHtbYo)  by0Ht 

H?  =  h[  -  H;sI,n(slY»H;s*,y,r'sZ,r<,H't  (48) 
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Here  the  various  S'-matrices  are  projection  matrices,  analogous  to  the  matrix  Sx  defined  above. 
The  final  term  in  our  approximation  (46),  p(Y° ,  Y  .  Y~  \  zt,  u1).  has  the  following  information 
matrix: 

Hf  =  (49) 

Putting  these  expressions  together  according  to  Equation  (46)  yields  the  following  information 
matrix,  in  which  the  landmark  Y°  is  now  indeed  deactivated: 

Ht  =  Hl-Hf  +  Hf 

_  TT  TTl  Q  (  OT  TTl  Q  1  qT  TTl 

~  Ht  ~  ^i^YoWYo^t^Yo)  O y0Ht 

+H'tSI,y.(Slr0H'tSx,yQ)-'SlYoHi 

-HAXSlH&X'SlH,  (50) 

The  resulting  information  vector  is  now  obtained  by  the  following  simple  consideration: 

bt  Ht 

=  fi(Ht-Ht  +  Ht) 

=  tfHt  +  tf(Ht-Ht) 

=  bt  +  tf{Ht-Ht)  (51) 

All  equations  can  be  computed  in  constant  time.  The  effect  of  this  approximation  is  the  de¬ 
activation  of  the  features  Y°,  while  introducing  only  new  links  between  active  features.  The 
sparsification  rule  requires  knowledge  of  the  mean  vector  //,  for  all  active  features,  which  is 
obtained  via  the  approximation  technique  described  in  the  previous  section.  From  (51),  it  is 
obvious  that  the  sparsification  does  not  affect  the  mean  fit,  that  is,  II,  lbj  =  '  ll,\  '[(v]7  -  Fur¬ 
thermore,  our  approximation  minimizes  the  KF  dovergence  to  the  correct  posterior.  These 
property  is  essential  for  the  consistency  of  our  approximation. 

The  sparsification  is  executed  whenever  a  measurement  update  of  a  motion  update  would 
violate  a  sparseness  constraint.  Active  features  are  chosen  for  deactivation  in  reverse  order 
of  the  magnitude  of  their  link.  This  strategy  tends  to  deactivate  features  whose  last  sighting 
is  furthest  away  in  time.  Empirically,  it  induces  approximation  errors  that  are  negligble  for 
appropriately  chosen  sparseness  constraints  9X  and  9y. 

4  Experimental  Results 

Our  present  experiments  are  preliminary:  They  only  rely  on  simulated  data,  and  they  require 
known  data  associations.  Our  primary  goal  was  to  compare  SEIFs  to  the  computationally  more 
cumbersome  EKF  solution  that  is  currently  in  widespread  use. 

An  example  situation  comparing  EKFs  with  our  new  filter  can  be  found  in  Figure  6.  This 
result  is  typical  and  was  obtained  using  a  sparse  information  matrix  with  9X  —  6,  9X  —  10,  and 
a  constant  time  implementation  of  coordinate  descent  that  updates  K  =  10  random  landmark 
estimates  in  addition  to  the  landmark  estimates  connected  to  the  robot  at  any  given  time.  The 
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Figure  6:  Comparison  of  EKFs  with  SEIFs  using  a  simulation  with  N  =  50  landmarks.  In  both  diagrams,  the  left 
panels  show  the  fi  nal  fi  Iter  result,  which  indicates  higher  certainties  for  our  approach  due  to  the  approximations 
involved  in  maintaining  a  sparse  information  matrix.  The  center  panels  show  the  links  (red:  between  the  robot  and 
landmarks;  green:  between  landmarks).  The  right  panels  show  the  resulting  covariance  and  normalized  information 
matrices  for  both  approaches.  Notice  the  similarity! 


17 


key  observation  is  the  apparent  similarity  between  the  EKF  and  the  SEIF  result.  Both  estimates 
are  almost  indistinguishable,  despite  the  fact  that  EKFs  use  quadratic  update  time  whereas  SEIF 
require  only  constant  time. 

We  also  performed  systematic  comparisons  of  three  algorithms:  EKFs,  SEIFs,  and  a  variant 
of  SEIFs  in  which  the  exact  state  estimate  fit  is  available.  The  latter  was  implemented  using 
matrix  inversion  (hence  does  not  run  in  constant  time).  It  allowed  us  to  tease  apart  the  error 
introduced  by  the  amortized  mean  recovery  step,  from  the  error  induced  through  sparsification. 
The  following  table  depicts  results  for  N  —  50  landmarks,  after  500  update  cycles,  at  which 
point  all  three  approaches  are  near  convergence. 


#  experiments 
(so  far) 

fi  nal  error 

(with  95%  conf.  interval) 

fi  nal  #  of  links 
(with  95%  conf.  interval) 

computation 
(per  update) 

EKF 

1,000 

(5.54  ±  0.67)  •  10~3 

1,275 

0(N'2) 

SEIF  with  exact  fit 

1,000 

(4.75  ±0.67)  •  10~3 

549  ±  1.60 

0(N3) 

SEIF  (constant  time) 

1,00 

(6.35  ±  0.67)  •  10“3 

549  ±  1.59 

0(1) 

As  these  results  suggest,  our  approach  approximates  EKF  very  tightly.  The  residual  map  er¬ 
ror  of  our  approach  is  with  6.35  •  10”3  approximately  14.6%  higher  than  that  of  the  extended 
Kalman  filter.  This  error  appears  to  be  largely  caused  by  the  coordinate  descent  procedure,  and 
is  possibly  inflated  by  the  fact  that  K  =  10  is  a  small  value  given  the  size  of  the  map.  Enforcing 
the  sparseness  constraint  seems  not  to  have  any  negative  effect  on  the  overall  error  of  the  result¬ 
ing  map,  as  the  results  for  our  sparse  filter  implementation  suggest.  However,  these  findings  are 
somewhat  premature  and  are  subject  to  an  ongoing  experimental  verification  using  real-world 
data. 


5  Discussion 

This  paper  proposed  a  constant  time  algorithm  for  the  SLAM  problem.  Our  approach  adopted 
the  information  form  of  the  EKF  to  represent  all  estimates.  Based  on  the  empirical  observation 
that  in  the  information  form,  most  elements  in  the  normalized  information  matrix  are  near¬ 
zero,  we  developed  a  sparse  extended  information  filter,  or  SEIF.  This  filter  enforces  a  sparse 
information  matrix,  which  can  be  updated  in  constant  time.  In  the  linear  SLAM  case,  all  updates 
can  be  performed  in  constant  time;  in  the  non-linear  case ,  additional  state  estimates  are  needed 
that  are  not  part  of  the  regular  information  form  of  the  EKF.  We  proposed  a  amortized  constant¬ 
time  coordinate  descent  algorithm  for  recovering  these  state  estimates  from  the  information 
form. 

The  approach  has  been  fully  implemented  and  compared  to  the  EKF  solution.  Overall,  we 
found  that  SEIFs  produce  results  that  differ  only  marginally  from  that  of  the  EKFs.  Given 
the  computational  advantages  of  SEIFs  over  EKFs,  we  believe  that  SEIFs  should  be  a  viable 
alternative  to  EKF  solutions  when  building  high-dimensional  maps. 

Our  approach  puts  a  new  perspective  on  the  rich  literature  on  hierarchical  mapping,  briefly 
outlined  in  the  introduction  to  this  paper.  Like  SEIF,  these  techniques  focus  updates  on  a  subset 
of  all  features,  to  gain  computational  efficiency.  SEIFs,  however,  composes  submaps  dynam¬ 
ically,  whereas  past  work  relied  on  the  definition  of  static  submaps.  We  conjecture  that  our 
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sparse  network  structures  capture  the  natural  dependencies  in  SLAM  problems  much  better 
than  static  submap  decompositions,  and  in  turn  lead  to  more  accurate  results.  They  also  avoid 
problems  that  frequently  occur  at  the  boundary  of  submaps,  where  the  estimation  can  become 
unstable.  However,  the  verification  of  these  claims  will  be  subject  to  future  research.  A  related 
paper  discusses  the  application  of  constant  time  techniques  to  information  exchange  problems 
in  multi-robot  SLAM  [21]. 
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Appendix:  Proofs 


Proof  of  Lemma  1 :  Measurement  updates  are  realized  via  (21)  and  (22),  restated  here  for  the 
reader’s  convenience: 

Ht  =  Ht  +  CtZ~lCj  (52) 

bt  =  bt  +  (zt  —  zt  +  Cj  Ht)T  Z  1Cj  (53) 

From  the  estimate  of  the  robot  pose  and  the  location  of  the  observed  feature,  the  prediction  zt 
and  all  non-zero  elements  of  the  Jacobian  Ct  can  be  calculated  in  constant  time,  for  any  of  the 
commonly  used  measurement  models  g.  The  constant  time  property  follows  now  directly  from 
the  sparseness  of  the  matrix  Ct,  discussed  already  in  Section  2.2.  This  sparseness  implies  that 
only  finitely  many  values  have  to  be  changed  when  transitioning  from  Ht  to  //,,  and  from  bt  to 
bt.  Q.E.D. 

Proof  of  Lemma  2:  For  At  =  0,  Equation  (34)  gives  us  the  following  updating  equation  for 
the  information  matrix: 

Ht  =  [H-_\  +  SxUtSl}-1  (54) 

Applying  the  matrix  inversion  lemma  1  leads  to  the  following  form: 

Ht  =  Ht-i  -Ht.tSxlUt1 +  SlHt.lSx]-1SlHt-i 

- - v - ' 

=-Lt 

=  Ht-i  -  Ht-iLt  (56) 

The  update  of  the  information  matrix,  Ht-\Lt,  is  a  matrix  that  is  non-zero  only  for  elements 
that  correspond  to  the  robot  pose  and  the  active  features.  To  see,  we  note  that  the  term  inside 
the  inversion  in  Lt  is  a  low-dimensional  matrix  which  is  of  the  same  dimension  as  the  motion 
noise  Ut.  The  inflation  via  the  matrices  Sx  and  ,5'J  leads  to  a  matrix  that  is  zero  except  for 
elements  that  correspond  to  the  robot  pose.  The  key  insight  now  is  that  the  sparseness  of  the 
matrix  Ht_ i  implies  that  only  finitely  many  elements  of  ///  i  L,  may  be  non-zero,  namely  those 
corresponding  to  the  robot  pose  and  active  features.  They  are  easily  calculated  in  constant  time. 

For  the  information  vector,  we  obtain  from  (34)  and  (56): 

bt  =  [bt-iH~_\  +  Aj]Ht 

=  [bt-iH^  +  Aj^Ht^-Ht^Lt) 

—  bt-i  +  —  bt-\Lt  +  A.J  Ht_iLt 

(57) 

As  above,  the  sparseness  of  Ht_  i  and  of  the  vector  At  ensures  that  the  update  of  the  information 
vector  is  zero  except  for  entries  corresponding  to  the  robot  pose  and  the  active  features.  Those 
can  also  be  calculated  in  constant  time.  Q.E.D. 

1The  inversion  lemma,  as  used  throughout  this  paper,  is  stated  as  follows: 

(H~1  +  SBSTy1  =  H-HS(B~l  +  STHSy1  StH  (55) 
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Proof  of  Lemma  3:  The  update  of  Ht  requires  the  definition  of  the  auxiliary  variable  T,  :  = 
(/  +  At)~l.  The  non-trivial  components  of  this  matrix  can  essentially  be  calculated  in  constant 
time  by  virtue  of: 

%  =  {I  +  S^AtSxSg)-1 

=  I  -  ISX(SXISTX  +  [STxAtSx]-')-'STxI 

=  I-Sx(I+lSlAtSx]-l)~lSTx  (58) 

Notice  that  Tq  differs  from  the  identity  matrix  /  only  at  elements  that  correspond  to  the  robot 
pose,  as  is  easily  seen  from  the  fact  that  the  inversion  in  (58)  involves  a  low-dimensional  matrix. 

The  definition  of  Tfi  allows  us  to  derive  a  constant- time  expression  for  updating  the  infor¬ 
mation  matrix  H : 

Ht  =  [(I  +  At)H^_\(I  +  Atf  +  SxUtSl}-1 

s — - - v - y 

1 

=  [(Hur'+SxUtszr1 

=  HU  -  HUSxpf1  +  sTxHUsx]~lsTxHU 

^ - ' 

=:A  Ht 

=  HU  -  A Ht  (59) 

The  matrix  HU  —  T/T/,-]  T,  is  easily  obtained  in  constant  time,  and  by  the  same  reasoning 
as  above,  the  entire  update  requires  constant  time.  The  information  vector  bt  is  now  obtained  as 
follows: 

bt  =  [bt-iHU  +  K]Ht 

=  bt^HUHt  +  AjHt 

=  bt-UUUt  +  Ht _!  -  Ht—\  +  HU  ~  HU)  +  ^Ht 

" - V - '  V - — - - ^ 

=0  =0 

=  bt^HUiHt-!  +  Ht-  HU  -Ht- 1  +  HU)  +  A JHt 

s,  y  ^ 

-A  Ht 

=  bt-iH-_\(Ht-i  -  A Ht  -  Ht _i  +  HU)  +  A jHt 
=  6t_!  -  bt^HUUHt  -  Ht_x  +  HU)  +  KHt 

=  bt-!  -  iiUHt-xHUUHt  -  Ht- 1  +  HU)  +  KU 

=  bt^  -  1-iJUAHt  ~  Ht-X  +  HU)  +  A^ Ht  (60) 

The  update  A Ht  is  non-zero  only  for  elements  that  correspond  to  the  robot  pose  or  active 
features.  Similarly,  the  difference  //'_ ,  —  //,  |  is  non-zero  only  for  constantly  many  elements. 
Therefore,  only  those  mean  estimates  in  Ht-\  are  necessary  to  calculate  the  product  //Jl ,  A//,. 
Q.E.D. 

Proof  of  Lemma  4:  The  mode  vt  of  (38)  is  given  by 

A  =  argmax  p(ut) 
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(61) 


=  argmax  exp  j  —  |z/fT  Htvt  +  bj  is  A 
=  argrnin  \isj Htist  -  b[ist 

Vt 

The  gradient  of  the  expression  inside  the  minimum  in  (61)  with  respect  to  vt  is  given  by 

\vjHtvt-b^vt }  =  Htist  -  bj  (62) 

whose  minimum  ist  is  attained  when  the  derivative  (62)  is  0,  that  is, 

vt  =  (63) 

From  this  and  Equation  (37)  it  follows  that  ist  =  /q .  Q.E.D. 


24 


